feat(app): add autoware module (gyro_odometer)#688
Conversation
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
nokosaaan
changed the title
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
|
This PR will be reviewed after #687 is merged. |
nokosaaan
changed the title
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
Signed-off-by: nokosaaan <nishimura.r.019@ms.saitama-u.ac.jp>
kobayu858
changed the title
| if !self.vehicle_twist_queue.is_empty() && !self.gyro_queue.is_empty() { | ||
| let latest_vehicle_twist_stamp = | ||
| self.vehicle_twist_queue.back().unwrap().header.timestamp; | ||
| let latest_imu_stamp = self.gyro_queue.back().unwrap().header.timestamp; | ||
|
|
||
| if Self::check_timeout( | ||
| current_time, | ||
| latest_vehicle_twist_stamp, | ||
| self.message_timeout_sec, | ||
| ) { | ||
| self.vehicle_twist_queue.clear(); | ||
| self.gyro_queue.clear(); | ||
| return Err(GyroOdometerError::TimeoutError(String::from( | ||
| "Vehicle twist message timeout", | ||
| ))); | ||
| } | ||
|
|
||
| if Self::check_timeout(current_time, latest_imu_stamp, self.message_timeout_sec) { | ||
| self.vehicle_twist_queue.clear(); | ||
| self.gyro_queue.clear(); | ||
| return Err(GyroOdometerError::TimeoutError(String::from( | ||
| "IMU message timeout", | ||
| ))); | ||
| } | ||
| } |
There was a problem hiding this comment.
The timeout check timing in concat_gyro_and_odometer differs from the original C++ implementation. The current implementation only performs the timeout check when both queues are non-empty. In the original C++, the last received timestamp is managed in a field independent of the queue (latest_vehicle_twist_ros_time_), and the timeout check is performed before the queue empty check. As a result, if one queue is empty and messages from the other source stop arriving, the timeout will not be detected. For example, even if IMU messages have been absent for 2 seconds, it will go unnoticed as long as gyro_queue is empty. Please fix this.
| for gyro in &mut self.gyro_queue { | ||
| let transformed_angular_velocity = tf.apply_to_vector(gyro.angular_velocity.clone()); | ||
| gyro.angular_velocity = transformed_angular_velocity; | ||
| } |
There was a problem hiding this comment.
In the original C++ implementation, angular_velocity_covariance should also be transformed via transform_covariance() after the angular_velocity vector transformation. This is currently omitted because the TF used in evaluation is a fixed identity transform, making the result identical. Please add this as a comment in the code.
| } | ||
| } | ||
|
|
||
| pub fn process_result( |
There was a problem hiding this comment.
Regarding the discrepancy between C++'s publish_data (which outputs 4 topics: TwistStamped raw, TwistWithCovarianceStamped raw, TwistStamped corrected, and TwistWithCovarianceStamped corrected) and the current process_result which returns only a single TwistWithCovarianceStamped: is this intentional because the other three outputs are not required in the minimal MRM DAG configuration? If so, please add a comment to the code stating that the remaining outputs are omitted as they are not needed in the current minimal MRM DAG configuration.
| Ok(Transform::identity()) | ||
| } else { | ||
| Ok(Transform::identity()) | ||
| } |
There was a problem hiding this comment.
This is understood given the architectural constraints. However, please leave a comment in the code noting that, in the original implementation, the queues should be cleared and the function should return when the transform lookup fails.
2 participants
Description
I added the
gyro_odometermodule.I have ported the Autoware test cases to Rust code.
I consolidated the header structures and other types previously defined separately in
imu_driverandvehicle_velocity_converterinto a common type and defined them in a separate file (common_types).Related links
https://github.com/autowarefoundation/autoware_core/tree/main/localization/autoware_gyro_odometer/test
How was this PR tested?
gyro_odometer.txt
Notes for reviewers